#!/usr/bin/env python3
import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
from agv_actions.action import Peripheral
from agv_msgs.msg import Action, ActionParam

class PeripheralActionClient(Node):
    def __init__(self):
        super().__init__('peripheral_action_client')
        self._action_client = ActionClient(self, Peripheral, 'do_peripheral')
        self.get_logger().info("Action client initialized")

    def send_goal(self, action_type, blocking_type="HARD", params=None):
        if not self._action_client.wait_for_server(timeout_sec=5.0):
            self.get_logger().error("Action server not available")
            return None

        action_params = []
        if params:
            for key, value in params.items():
                param = ActionParam()
                param.key = key
                param.value = str(value)
                param.valuedatatype = type(value).__name__
                action_params.append(param)

        goal_action = Action()
        goal_action.actiontype = action_type
        goal_action.actionid = f"{action_type}_001"
        goal_action.actiondescription = f"{action_type} operation"
        goal_action.blockingtype = blocking_type
        goal_action.actionparameters = action_params

        goal_msg = Peripheral.Goal()
        goal_msg.goal_action = goal_action

        self.get_logger().info(f"Sending {action_type} action...")
        return self._action_client.send_goal_async(goal_msg, feedback_callback=self.feedback_callback)

    def feedback_callback(self, feedback_msg):
        feedback = feedback_msg.feedback
        self.get_logger().info(
            f"Feedback: State={feedback.state}, "
            f"Progress={feedback.percent_complete:.1f}%")

    def result_callback(self, future):
        goal_handle = future.result()
        
        if not goal_handle.accepted:
            self.get_logger().error("Goal rejected by server")
            return

        result_future = goal_handle.get_result_async()
        rclpy.spin_until_future_complete(self, result_future)
        
        if result_future.result() is not None:
            result = result_future.result().result
            if result.success == 0:
                self.get_logger().info(f"Action succeeded: {result.message}")
            else:
                self.get_logger().error(f"Action failed (Code {result.success}): {result.message}")
        else:
            self.get_logger().error("Exception while getting result: {result_future.exception()}")

def main(args=None):
    rclpy.init(args=args)
    action_client = PeripheralActionClient()

    try:
        # 示例动作
        future = action_client.send_goal(
            action_type="pick",
            blocking_type="HARD",
            params={"height": 1.5, "speed": 0.2}
        )
        
        # 等待目标接受
        rclpy.spin_until_future_complete(action_client, future)
        
        # 处理结果
        action_client.result_callback(future)

    except KeyboardInterrupt:
        action_client.get_logger().info("Operation canceled by user")
    finally:
        action_client.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()